#pragma config(Sensor, in1,    pot,                 sensorPotentiometer)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  TouchRight,          sensorTouch)
#pragma config(Sensor, dgtl6,  TouchLeft,           sensorTouch)
#pragma config(Sensor, dgtl7,  Arm_limit,           sensorTouch)
#pragma config(Sensor, dgtl8,  Arm_closed,          sensorTouch)
#pragma config(Sensor, dgtl11,  Extend_limit,        sensorTouch)
#pragma config(Sensor, dgtl12, Retract_limit,       sensorTouch)
#pragma config(Motor,  port2,           DriveR,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveL,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           SuckUpper,     tmotorNormal, openLoop)
#pragma config(Motor,  port5,           ArmR,          tmotorNormal, openLoop)
#pragma config(Motor,  port6,           ArmL,          tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           SuckLower,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           ExtendR,       tmotorNormal, openLoop)
#pragma config(Motor,  port9,           ExtendL,       tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(60)
#pragma userControlDuration(200)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "Zeta_Function_Library_v1.1.c"

int arm_grounded;
int low_lock_point;
int high_lock_point;
int pre_auton_complete = 0;

void pre_auton()
{
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
  arm_grounded = SensorValue[pot];
  low_lock_point = arm_grounded + 800; //low goal potentiometer reading
                      //sets the low lock point based on the starting potentiometer point
  high_lock_point = arm_grounded + 1100; //high goal potentiometer reading
  arm_grounded += 100;
  pre_auton_complete = 1;
}

task autonomous()
{
  pre_auton();
  /*int arm_in_position = 0;  //arm is down; 0 for false and 1 for true
  while(arm_in_position != 1) {
	  arm_in_position = lock(high_lock_point);
	}
	drive_straight(50, 2);
	arm_in_position = 0;
	while(arm_in_position != 1) {
	  arm_in_position = lock(high_lock_point - 250);  //lower arm to scoring position
	}
	sucker(127);
	drive_straight(-50, 30);  //reverse, back away from goal
	wait1Msec(500);
	drive_straight(80, 3);
	wait1Msec(500);
	turn(100, 90);
	arm_in_position = 0;
	while(arm_in_position != 1) {
	  arm_in_position = lock(arm_grounded);
	}
	arm_in_position = 0;*/
	drive_to_touch(-70);
	wait1Msec(2000);
	drive_straight(40, 2);
	wait1Msec(1000);
	arm_vertical();
	wait1Msec(1000);
	drive_straight(-127, 3);
	wait1Msec(3000);
	pull_up();
  stop_all();
}
//Control Tweaking
	//DriveTrain
/*int scale(int input)          // for drive train scale v2
		{
		  if(input<=64&&input>=-64)     //if between -64 and 64, run half power
		    return .5*input;
		  if(input>=-96&&input<-64)                 // -64>input>=-96, run at (.7)-32 power
		    return .7*input-32;
		  if(input<=96&&input>64)                 // 64<input<=96, run at (.7)+32 power
		    return .7*input+32;
		  if(input<-96)                 // if less than -96, run at (29/32)-99 power ---> full power
		    return 29/32*input-99;
		  if(input>96)                  // if greater than 96, run at (28/31)+99 power ---> full power
		    return 28/31*input+99;
		  return 0;
		}*/

//USER CONTROL/////////////////////////////////////////////////////////////////
task usercontrol()
{
  /*if(pre_auton_complete != 1) {
    pre_auton();
  }*/
  int goal_lock = 0;  //0 for unlocked, -1 for low gal, 1 for high goal
  //calibration for potentiometer setting the arm
  //int low_lock_point = SensorValue[in1] + 703; //low goal potentiometer reading
                      //sets the low lock point based on the starting potentiometer point
  //int high_lock_point = SensorValue[in1] + 1000; //high goal potentiometer reading
  //end calibrarion
 while(true)
 {
  //Arm/Curl//////////////////////////////////////////////////////////////////////////
	  if(vexRT[Btn8L] == 1)//auto button close loop
	  {
	    goal_lock = -1;   //sets to low lock
	  }
	  else if(vexRT[Btn8R] == 1) {
	    goal_lock = 1;    //sets to high lock
	  }

	  if(goal_lock == -1)
	    lock(low_lock_point); //brings to low lock point
	  else if(goal_lock == 1)
      lock(high_lock_point);//moves arm to high lock point

	    switch(vexRT[Btn6U] - vexRT[Btn6D]) //manual arm control
			{
				case  1:goal_lock = 0;  //unlocks arm
				        motor[port5] = -127;
						    motor[port6] = -127;
				        break;
				case -1:goal_lock = 0;  //unlocks arm
				        if(SensorValue[Arm_closed] == 0) {
				          motor[port5] = 127;
						      motor[port6] = 127;
						    }
						    else {
						      motor[port5] = 0;
						      motor[port6] = 0;
						    }
				        break;
				case  0:if(goal_lock == 0 && SensorValue[in1] < high_lock_point + 500) { //past 3500, motors will not "tick"
				          motor[port5] = -10;
						      motor[port6] = -10;
					        break;
					    }
	    }
	  //}
	  //Tube Sucker/////////////////////////////////////////////////////////////////////
		  //Rotate up and down
			  if (vexRT[Btn7U] - vexRT[Btn7D])
			  {
					switch(vexRT[Btn7D] - vexRT[Btn7U])
					{
						case  1:motor[port4] = -127;
						        motor[port7] = -127;
								    break;
						case -1:motor[port4] = 127;
						        motor[port7] = 127;
						        break;
						case  0:motor[port4] = 0;
						        motor[port7] = 0;
						        break;

					}
		   	}
			//Suck in/Spit out
			  else
			  {
					switch(vexRT[Btn5D] - vexRT[Btn5U])
					{
						case  1:motor[port4] = 127; //suck in
						        motor[port7] = -127;
						        break;
						case -1:motor[port4] = -127;//shoot out
						        motor[port7] = 127;
						        break;
						case  0:motor[port4] = 0;
						        motor[port7] = 0;
						        break;

			    }
	      }
		//Extend///////////////////////////////////////////////////////////////////////
			switch(vexRT[Btn8D] - vexRT[Btn8U])
			{
				case  1:if(SensorValue[Retract_limit] != 0) {
				          motor[port8] = -127;  //retracts
				          motor[port9] = -127;
				        }
				        else {
				          motor[port8] = 0;
				          motor[port9] = 0;
				        }
				        break;
				case -1:if(SensorValue[Extend_limit] == 0) {
        				  motor[port8] = 127; //extends
				          motor[port9] = 127;
				        }
				        else {
				          motor[port8] = 0;
				          motor[port9] = 0;
				        }
				        break;
				case  0:motor[port8] = 0;
				        motor[port9] = 0;
				        break;
			}

   //Drive Train//////////////////////////////////////////////////////////////////
			//Right Drive
				if(vexRT[Ch2] < 15 && vexRT[Ch2] > -15)//If input is between 15 & -15...
				{
				  motor[port2] = 0;                    //...drive train motors = 0
				}
				else                                   //If input is greater then 15 or smaller then -15...
				{
				  //motor[port2] = scale(-vexRT[Ch2]);   //...drive trian motors = scale
				  motor[port2] = -vexRT[Ch2];
				}
		  //Left Drive
				if (vexRT[Ch3] < 15 && vexRT[Ch3] > -15)//If input is between 15 & -15...
				{
				  motor[port3] = 0;                     //...drive train motors = 0
				}
				else                                    //If input is greater then 15 or smaller then -15...
				{
				  //motor[port3] = scale(-vexRT[Ch3]);    //...drive trian motors = scale
				  motor[port3] = -vexRT[Ch3];
				}
  }//while
}//taskmain
